#include "zf_driver_flash.h"
#include "flash_driver.h"
#include "leg_control.h"
#include "pid_balence.h"
#include "camera.h"
flash_data_union_struct flash;

void save_all_data(void)
{
    flash.pid_loc_v_angle_kp.float_type    =   pid_loc_v_angle.kp;
    flash.pid_loc_v_angle_ki.float_type    =   pid_loc_v_angle.ki;
    flash.pid_loc_v_angle_kd.float_type    =   pid_loc_v_angle.kd;
 
    flash.pid_loc_a_angle_kp.float_type    =   pid_loc_a_angle.kp;
    flash.pid_loc_a_angle_ki.float_type    =   pid_loc_a_angle.ki;
    flash.pid_loc_a_angle_kd.float_type    =   pid_loc_a_angle.kd;
 
    flash.pid_inc_v_avarage_kp.float_type  = pid_inc_v_avarage.kp;
    flash.pid_inc_v_avarage_ki.float_type  = pid_inc_v_avarage.ki;
    flash.pid_inc_v_avarage_kd.float_type  = pid_inc_v_avarage.kd;
 
    flash.pid_loc_yaw_angle_kp.float_type  = pid_loc_yaw_angle.kp;
    flash.pid_loc_yaw_angle_ki.float_type  = pid_loc_yaw_angle.ki;
    flash.pid_loc_yaw_angle_kd.float_type  = pid_loc_yaw_angle.kd;

    flash.pid_inc_roll_angle_kp.float_type = pid_inc_roll_angle.kp;
    flash.pid_inc_roll_angle_ki.float_type = pid_inc_roll_angle.ki;
    flash.pid_inc_roll_angle_kd.float_type = pid_inc_roll_angle.kd;


    flash.split_point.int32_type = brightness_split_point;
    flash.BLACK_THRESHOLD.int32_type = ABSOLUTE_BLACK_THRESHOLD;
    flash.C_bright.int32_type =const_C_bright;
    flash.C_dark.int32_type =const_C_dark;
    flash.avr_speed.float_type = set_speed;

    flash.sever_left_up.uint32_type     = chassis.left_leg.up.duty_mid;
    flash.sever_left_down.uint32_type   = chassis.left_leg.down.duty_mid;
    flash.sever_right_up.uint32_type    = chassis.right_leg.up.duty_mid;
    flash.sever_right_down.uint32_type  = chassis.right_leg.down.duty_mid;


    flash_write_page(0, 127, (uint32*)&flash, sizeof(flash_data_union_struct) / sizeof(flash_data_union));
}

void read_all_data(void)
{
    flash_read_page(0, 127, (uint32*)&flash, sizeof(flash_data_union_struct) / sizeof(flash_data_union));
    pid_loc_v_angle.kp =  flash.pid_loc_v_angle_kp.float_type    ;
    pid_loc_v_angle.ki =  flash.pid_loc_v_angle_ki.float_type    ;
    pid_loc_v_angle.kd =  flash.pid_loc_v_angle_kd.float_type    ;
    pid_loc_a_angle.kp =  flash.pid_loc_a_angle_kp.float_type    ;
    pid_loc_a_angle.ki =  flash.pid_loc_a_angle_ki.float_type    ;
    pid_loc_a_angle.kd =   flash.pid_loc_a_angle_kd.float_type    ;
    pid_inc_v_avarage.kp = flash.pid_inc_v_avarage_kp.float_type;
    pid_inc_v_avarage.ki = flash.pid_inc_v_avarage_ki.float_type;
    pid_inc_v_avarage.kd = flash.pid_inc_v_avarage_kd.float_type;
    pid_loc_yaw_angle.kp = flash.pid_loc_yaw_angle_kp.float_type;
    pid_loc_yaw_angle.ki = flash.pid_loc_yaw_angle_ki.float_type;
    pid_loc_yaw_angle.kd = flash.pid_loc_yaw_angle_kd.float_type;
    pid_inc_roll_angle.kp = flash.pid_inc_roll_angle_kp.float_type ;
    pid_inc_roll_angle.ki = flash.pid_inc_roll_angle_ki.float_type ;
    pid_inc_roll_angle.kd = flash.pid_inc_roll_angle_kd.float_type ;
    brightness_split_point = flash.split_point.int32_type;
    ABSOLUTE_BLACK_THRESHOLD = flash.BLACK_THRESHOLD.int32_type;
    const_C_bright = flash.C_bright.int32_type;
    const_C_dark = flash.C_dark.int32_type;
    set_speed = flash.avr_speed.float_type;
    chassis.left_leg.up.duty_mid    =    flash.sever_left_up.uint32_type   ;
    chassis.left_leg.down.duty_mid    =flash.sever_left_down.uint32_type ;
    chassis.right_leg.up.duty_mid    =flash.sever_right_up.uint32_type  ;
    chassis.right_leg.down.duty_mid    =flash.sever_right_down.uint32_type;
}


